#pragma once
#ifndef DRRT_H
#define DRRT_H

#include "basic_typedef.h"
#include "Kd_tree_d.h"
#include "Prm.h"
#include "MultiRobotPRM.h"
#include "Graph.h"
#include "DRRTLocalConnector.h"

struct ConfTree
{
    Kd_tree_d<Kernel_d> tree;
    Graph<Point_d, cmpPoint_d> graph;
    list<Point_d> arr;
} ;

class DRRT
{
public:
    DRRT(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room);
    ~DRRT(void);

    bool generate_roadmap(int number_vertices = 300, int k_nearest = 12, double eps = 1);
    void update_path(vector<vector<Conf>> &path, int N = 20, int P = 2);

private:

    void expand_tree(ConfTree &tree, int N);

    list<Point_d> connect_trees(ConfTree &tree1, ConfTree &tree2, int P);

    Point_d generate_sample();

    void retrive_path(vector<vector<Conf>> &path, ConfTree &tree1, ConfTree &tree2, list<Point_d> &pi);

    Point_d direction_oracle(Point_d &q_near, Point_d &q_rand);

    vector<Polygon_2>           &m_robot_polygons;
    int                         m_robot_num;
    Obstacles                   &m_obstacles;
    vector<Conf>                &m_start_confs, &m_target_confs;
    Room                        &m_room;
    double                      m_eps;

    vector<Prm*>                m_prm;
    Point_d                     m_start, m_target;

    MultiRobotCollisionDetector m_mr_collision_detector;
    map<Point_d, vector<Conf>, cmpPoint_d>  m_map;      // Reverse maping...
};

#endif